// License: Apache 2.0. See LICENSE file in root directory.
// Copyright(c) 2019 Intel Corporation. All Rights Reserved.
#include "../include/realsense_node_factory.h"
#include <nav_msgs/Odometry.h>
#include <ros/ros.h> 
#include <iostream>
#include <boost/bind.hpp>



void pose_callback(std::shared_ptr<rs2::pipeline> pipe,ros::Publisher &t265_sdx_pub,nav_msgs::Odometry &pose_msg){
    static int count = 0;
    auto frames = pipe->wait_for_frames();
    auto f = frames.first_or_default(RS2_STREAM_POSE);
    auto pose = f.as<rs2::pose_frame>().get_pose_data();
    pose_msg.header.stamp = ros::Time::now ();
    pose_msg.header.frame_id = "camera_odom_frame";
    pose_msg.child_frame_id = "camera_pose_frame";
    pose_msg.pose.pose.position.x = -pose.translation.z;
    pose_msg.pose.pose.position.y = -pose.translation.x;
    pose_msg.pose.pose.position.z = pose.translation.y;
    pose_msg.pose.pose.orientation.x = -pose.rotation.z;
    pose_msg.pose.pose.orientation.y = -pose.rotation.x;
    pose_msg.pose.pose.orientation.z = pose.rotation.y;
    pose_msg.pose.pose.orientation.w = pose.rotation.w;
    pose_msg.twist.twist.angular.x = -pose.angular_velocity.z;
    pose_msg.twist.twist.angular.y = -pose.angular_velocity.x;
    pose_msg.twist.twist.angular.z = pose.angular_velocity.y;
    pose_msg.twist.twist.linear.x = -pose.velocity.z;
    pose_msg.twist.twist.linear.y = -pose.velocity.x;
    pose_msg.twist.twist.linear.z = pose.velocity.y;

    double cov_pose(0.01 * pow(10, 3-(int)pose.tracker_confidence));
    double cov_twist(0.01 * pow(10, 1-(int)pose.tracker_confidence));
    double track_con(-1.0);
    double map_con(-1.0);

    switch((int)(pose.tracker_confidence)){
        case 0:
            track_con = 0.0;
            break;
        case 1:
            track_con = 1.0;
            break;
        case 2:
            track_con = 2.0;
            break;
        case 3:
            track_con = 3.0;
            break;
        default:
            break;
    }

    switch((int)(pose.mapper_confidence)){
        case 0:
            map_con = 0.0;
            break;
        case 1:
            map_con = 1.0;
            break;
        case 2:
            map_con = 2.0;
            break;
        case 3:
            map_con = 3.0;
            break;
        default:
            break;
    }

    // 用于保存地图可信度，追踪可信度
    pose_msg.pose.covariance = {track_con, 0, 0, 0, 0, 0,
                                0, track_con, 0, 0, 0, 0,
                                0, 0, track_con, 0, 0, 0,
                                0, 0, 0, map_con, 0, 0,
                                0, 0, 0, 0, map_con, 0,
                                0, 0, 0, 0, 0, map_con};

    pose_msg.twist.covariance = {cov_pose, 0, 0, 0, 0, 0,
                                 0, cov_pose, 0, 0, 0, 0,
                                 0, 0, cov_pose, 0, 0, 0,
                                 0, 0, 0, cov_twist, 0, 0,
                                 0, 0, 0, 0, cov_twist, 0,
                                 0, 0, 0, 0, 0, cov_twist};

    ROS_INFO_THROTTLE(0.5,
                      "Device Position: %2.6f,%2.6f,%2.6f",
                      pose_msg.pose.pose.position.x,pose_msg.pose.pose.position.y,pose_msg.pose.pose.position.z
    );
    count ++;
    if(count%4==0){
        t265_sdx_pub.publish(pose_msg);
        count = 0;
    }
}

int main(int argc, char * argv[]) try
{
    ros::init(argc, argv, "t265_sdx");
    ros::NodeHandle nh;
    ros::Publisher t265_sdx_pub;
    nav_msgs::Odometry pose_msg;
    //发布主题
    t265_sdx_pub = nh.advertise<nav_msgs::Odometry>("/camera/odom/sample", 100);
    ros::Rate loop_rate(210);
    auto pipe = std::make_shared<rs2::pipeline>();
    rs2::config cfg;

    cfg.enable_stream(RS2_STREAM_POSE, RS2_FORMAT_6DOF);
    pipe->start(cfg);
    std::thread _query_thread;
    rs2::device _device = pipe->get_active_profile().get_device();
    rs2::context _ctx;
    rs2::device_list devices = _ctx.query_devices();
    ros::Timer fast_timer = nh.createTimer(ros::Duration(1/210),boost::bind(pose_callback,pipe,t265_sdx_pub,pose_msg));

    ros::AsyncSpinner spinner(2);
    spinner.start();
    ros::waitForShutdown();
    // Main loop
    return EXIT_SUCCESS;
}

catch (const rs2::error & e)
{
    std::cerr << "RealSense error calling " << e.get_failed_function() << "(" << e.get_failed_args() << "):\n    " << e.what() << std::endl;
    return EXIT_FAILURE;
}
catch (const std::exception& e)
{
    std::cerr << e.what() << std::endl;
    return EXIT_FAILURE;
}
